#include "component.h"

Component::Component()
{
    cloud.reset(new PointCloud<PointXYZRGB>);
    componentIndices.reset(new PointIndices);
    generatingIndex  = 0;
    clusterThreshold = 0;
    colorThreshold = 0;
}

Component::Component(PointCloud<PointXYZRGB>::Ptr cloud_, PointIndices::Ptr componentIndices_, int generatingIndex_, double clusterThreshold_, int colorThreshold_)
{
    cloud = cloud_;
    componentIndices=componentIndices_;
    generatingIndex=generatingIndex_;
    clusterThreshold=clusterThreshold_;
    colorThreshold=colorThreshold_;
}

Component::Component(PointCloud<PointXYZRGB>::Ptr cloud_, PointIndices::Ptr componentIndices_)
{
    cloud = cloud_;
    componentIndices=componentIndices_;
    generatingIndex = 0;
    clusterThreshold = 0;
    colorThreshold = 0;
}

//GETTERS
PointCloud<PointXYZRGB>::Ptr Component::getCloud()
{
    return cloud;
}

PointIndices::Ptr Component::getIndices()
{
    return componentIndices;
}

PointCloud<PointXYZRGB>::Ptr Component::getComponentCloud()
{
    PointCloud<PointXYZRGB>::Ptr componentCloud(new PointCloud<PointXYZRGB>);
    copyPointCloud(*cloud,*componentIndices,*componentCloud);
    return componentCloud;
}

int Component::getGeneratingIndex()
{
    return generatingIndex;
}

int Component::getColorThreshold()
{
    return colorThreshold;
}

double Component::getClusterThreshold()
{
    return clusterThreshold;
}

int Component::getColR()
{
    return cloud->at(generatingIndex).r;
}

int Component::getColG()
{
    return cloud->at(generatingIndex).g;
}

int Component::getColB()
{
    return cloud->at(generatingIndex).b;
}

float Component::getPosX()
{
    return cloud->at(generatingIndex).x;
}

float Component::getPosY()
{
    return cloud->at(generatingIndex).y;
}

float Component::getPosZ()
{
    return cloud->at(generatingIndex).z;
}

int Component::getSize()
{
    return componentIndices->indices.size();
}

Eigen::Vector3f Component::getAverageColor()
{
    // Copy the cloud (NON NECESSARIO, SI POTREBBE DIRETTAMENTE LEGGERE I VALORI DALLA CLOUD ORIGINALE AVENDO GLI INDICI, DA SISTEMARE)
    PointCloud<PointXYZRGB>::Ptr rgbCloud(new PointCloud<PointXYZRGB>);
    copyPointCloud(*cloud,*componentIndices,*rgbCloud);
    // Calculate the average
    Eigen::Vector3i averageRGBColor = rgbCloud->points.at(0).getRGBVector3i();
    for(size_t i = 1; i < rgbCloud->size(); i++)
        averageRGBColor += rgbCloud->points.at(i).getRGBVector3i();
    averageRGBColor = averageRGBColor/rgbCloud->size();
    // Convert in HSV format
    PointXYZRGB rgbPoint(averageRGBColor[0], averageRGBColor[1], averageRGBColor[2]);
    //    PointXYZHSV hsvPoint;
    //    PointXYZRGBtoXYZHSV(rgbPoint, hsvPoint); // non funziona con PCL 1.5, dovrebbe andare con versioni successive, se risultasse questo il problema, aggiorno
    Eigen::Vector3f hsvColor;
    //    hsvColor[0] = hsvPoint.h;
    //    hsvColor[1] = hsvPoint.s;
    //    hsvColor[2] = hsvPoint.v;
    hsvColor[0] = rgbPoint.r;
    hsvColor[1] = rgbPoint.g;
    hsvColor[2] = rgbPoint.b;
    return hsvColor;
}

PointXYZRGB Component::getCenterOfMass()
{
    Eigen::Vector4f centroid;
    compute3DCentroid(*getComponentCloud(), centroid);
    PointXYZRGB centerOfMass;
    centerOfMass.x=centroid[0];
    centerOfMass.y=centroid[1];
    centerOfMass.z=centroid[2];
    return centerOfMass;
}


Eigen::VectorXf
Component::getPlaneCoefficients ()
{
    Eigen::VectorXf medianCoefficients(4);
    Eigen::VectorXf finalCoefficients(4);
    pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal>* ne = new pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal>();
    pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());
    ne->setInputCloud(cloud);
    ne->setIndices(componentIndices);
    ne->setSearchMethod(tree);
    ne->setRadiusSearch(2); // use all neighbor points in a sphere of radius 2mm to compute every normal
    ne->compute(*normals); // compute the normals and then calculate the average normal from them.

    //1st loop to find median (and imprecise) plane coefficients
    for(size_t i = 0; i < normals->size(); i++)
    {
        Eigen::VectorXf thisNormal=normals->at(i).getNormalVector4fMap();
        medianCoefficients = medianCoefficients + thisNormal;
    }
    medianCoefficients = medianCoefficients/normals->size();

    //2nd loop to find precise plane coefficients
    int inliers=0;
    for(size_t i = 0; i < normals->size(); i++)
    {
        Eigen::VectorXf thisNormal= normals->at(i).getNormalVector4fMap();
        float cosine=(  (thisNormal.dot(medianCoefficients)) / (thisNormal.norm() * medianCoefficients.norm())  );
        if(cosine>0.8) //cos >0.8 vuol dire circa scostamento di 30-35°
        {
            finalCoefficients =finalCoefficients + thisNormal;
            inliers++;
        }
    }
    finalCoefficients = finalCoefficients/inliers;
    cout << "PlaneCoefficients:\n"<<finalCoefficients<<endl;
    return finalCoefficients;
}

//float Component::getRadiusToCenterOfMass()
//{
//    vector<float> all_radius;
//    PointXYZRGB center= getCenterOfMass();
//    PointCloud<PointXYZRGB>::Ptr tempCloud(new PointCloud<PointXYZRGB>);
//    tempCloud=getComponentCloud();
//    for (int i=0; i<tempCloud->size();i++)
//    {
//        float radius_ith = (center.getVector3fMap() - tempCloud->at(i).getVector3fMap()).norm();
//        all_radius.push_back(radius_ith);
//    }
//    sort(all_radius.begin(), all_radius.end());
//    float sum = 0;
//    for (int i = all_radius.size() - all_radius.size()*0.9; i < all_radius.size(); i++)
//    {
//        sum += all_radius.at(i);
//    }
//    return sum / all_radius.size()*0.9;
//}

float Component::getRadiusToCenterOfMass()
{
    vector<float> all_radius;
    PointXYZRGB center= getCenterOfMass();
    PointCloud<PointXYZRGB>::Ptr tempCloud(new PointCloud<PointXYZRGB>);
    tempCloud=getComponentCloud();
    for (int i=0; i<tempCloud->size();i++)
    {
        float radius_ith = ( center.getVector3fMap() - tempCloud->at(i).getVector3fMap() ).norm();
        all_radius.push_back(radius_ith);
    }
    sort(all_radius.begin(),all_radius.end());
    float sum=0;
    for (int i=all_radius.size()-10; i<all_radius.size();i++)
    {
        sum+=all_radius.at(i);
    }
    return sum/10;
}











